//
// Created by PulsarV on 18-5-9.
//

#ifndef ROBOCAR_RCMOVE_H
#define ROBOCAR_RCMOVE_H

#include <rc_cv/rcCV.h>
#include <rc_log/rclog.h>
#include <rc_globalVarable/rc_global_wheel.h>
#include <rc_move/rcmove_data_struct.h>
#include <rc_system/data_struct.h>
#include <rc_system/config.h>
#include <rc_system/context.h>
#include <mraa/common.hpp>
#include <mraa/gpio.hpp>
#include <mraa/pwm.hpp>
#include "rc_task/rcBaseTask.h"

namespace rccore {
    namespace move {
        class RobotCarMove : public task::BaseTask {
        public:
            explicit RobotCarMove(std::shared_ptr<common::Context> pcontext);

            /**
             * 初始化车轮
             * @param moveDevice
             */
            void init();

            void set_value(std::shared_ptr<mraa::Gpio> pgpio, int value);

            void dir_pin(std::shared_ptr<mraa::Gpio> pgpio);

            /**
             * 直接命令
             * @param com
             * @param speed_1
             * @param speed_2
             * @param speed_3
             */
            void command(char com, float speed_1, float speed_2, float speed_3);

            void excute(WHEEL_DATA &wheelData);

            void wheel_1_forward(float speed_v_m_s);

            void wheel_1_backward(float speed_v_m_s);

            void wheel_2_forward(float speed_v_m_s);

            void wheel_2_backward(float speed_v_m_s);

            void wheel_3_forward(float speed_v_m_s);

            void wheel_3_backward(float speed_v_m_s);

            void convert_speed(float speed_v_m_s, const std::shared_ptr<mraa::Pwm> &pwm_port);

            /**
             * 前进
             * @param speed_1 1轮轮速
             * @param speed_2 2轮轮速
             */
            void wheel_go_forward(float speed_1_v_m_s, float speed_2_v_m_s);

            /**
             * 后退
             * @param speed_1 1轮轮速
             * @param speed_2 2轮轮速
             */
            void wheel_go_backward(float speed_1_v_m_s, float speed_2_v_m_s);

            /**
             * PWM停止
             */
            void wheel_stop();

            /**
             * PWM开始
             */
            void wheel_start();

            /**
             * 顺时针旋转
             * @param speed_1 1轮轮速
             * @param speed_2 2轮轮速
             * @param speed_3 3轮轮速
             */
            void wheel_CW(float speed_1_v_m_s, float speed_2_v_m_s, float speed_3_v_m_s);

            /**
             * 逆时针旋转
             * @param speed_1 1轮轮速
             * @param speed_2 2轮轮速
             * @param speed_3 3轮轮速
             */
            void wheel_AC(float speed_1_v_m_s, float speed_2_v_m_s, float speed_3_v_m_s);

            /**
             * 陀螺仪校准
             */
            void wheel_check(int times);//校速

            //编码控制数据
            char *decode(
                    bool weel_1_direction, unsigned short wheel_1_speed,
                    bool weel_2_direction, unsigned short wheel_2_speed,
                    bool weel_3_direction, unsigned short wheel_3_speed
            );

            ~RobotCarMove();

        private:
            int type = 0;
            bool AutoMove = false;
            bool AutoFollow = false;
            std::shared_ptr<mraa::Pwm> _pwm_1;
            std::shared_ptr<mraa::Pwm> _pwm_2;
            std::shared_ptr<mraa::Pwm> _pwm_3;
            std::shared_ptr<mraa::Gpio> _gpio_1;
            std::shared_ptr<mraa::Gpio> _gpio_2;
            std::shared_ptr<mraa::Gpio> _gpio_3;
//        mraa::Pwm m_pwm_1;
#ifdef USE_DSP_DEVICE
            libserv::Serial serial_device;
#endif
        };
    }
}


#endif //ROBOCAR_RCMOVE_H
